/*
 * ============================================================================
 *
 *       Filename:  algorithm.c
 *
 *    Description:  
 *
 *        Version:  1.0
 *        Created:  2014年09月06日 09时30分44秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  jianxi sun (jianxi), ycsunjane@gmail.com
 *   Organization:  
 *
 * ============================================================================
 */
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#define DEBUG
#include "log.h"
#include "algorithm.h"

debug = 1;

struct matrix_A_t {
	double a[2][2];
};

struct matrix_A_1_t {
	double a[2][2];
};

struct matrix_B_t{
	double a[2];
};

struct x_t {
	double x;
	double y;
};

static struct matrix_A_t A;
static struct matrix_A_1_t A_1;
struct matrix_B_t B;
static struct x_t X;

static int set_matrix_A_B(struct point_t *pi)
{
	int i, num = 2;
	for(i = 0; i < num; i++) {
		/* set A */
		A.a[i][0] = pi[i].x - pi[num].x;
		A.a[i][1] = pi[i].y - pi[num].y;
		sys_debug("%lf = %lf - %lf\n", 
			A.a[i][0], pi[i].x, pi[num].x);
		sys_debug("%lf = %lf - %lf\n", 
			A.a[i][1], pi[i].y, pi[num].y);

		/* set B */
		B.a[i] =  pow(pi[i].x, 2) - pow(pi[num].x, 2)
			+ pow(pi[i].y, 2) - pow(pi[num].y, 2)
			+ pow(pi[num].d, 2) - pow(pi[i].d, 2);
	}

	sys_debug("m: %d\n", num);
	sys_debug("A: \n");
	for(i = 0; i < num; i++) {
		sys_debug("%lf %lf\n", A.a[i][0], A.a[i][1]);
	}

	sys_debug("B: \n");
	for(i = 0; i < num; i++)
		sys_debug("%lf\n", B.a[i]);

	return num;
}

static int cal_A_1(int m)
{
	double denom = A.a[0][0] * A.a[1][1]
		- A.a[0][1] * A.a[1][0];
	if(denom == 0) {
		sys_debug("demon == 0\n");
		return -1;
	}
	A_1.a[0][0] = A.a[1][1] / denom;
	A_1.a[0][1] = -A.a[0][1] / denom;
	A_1.a[1][0] = -A.a[1][0] / denom;
	A_1.a[1][1] = A.a[0][0] / denom;

	sys_debug("A_1: %d\n", m);
	sys_debug("%lf %lf\n%lf %lf\n", 
		A_1.a[0][0], A_1.a[0][1], 
		A_1.a[1][0], A_1.a[1][1]);
	return 0;
}

static void __get_x()
{
	int i;
	X.x = 0;
	X.y = 0;
	for(i = 0; i < 2; i++) {
		X.x += A_1.a[0][i] * B.a[i];
		X.y += A_1.a[1][i] * B.a[i];
		sys_debug("%lf += %lf * %lf\n", 
			X.x, A_1.a[0][i], B.a[i]);
		sys_debug("%lf += %lf * %lf\n", 
			X.y, A_1.a[1][i], B.a[i]);
	}
	X.x /= 2;
	X.y /= 2;
}

int get_point(struct point_t *pi, int num, double *x, double *y)
{
	int i, m, total, ret;
	struct point_t *_pi = pi;
	*x = *y = 0;

	total = num - 2;
	for(i = 0; i < num - 2; i++) {
retry:
		_pi = pi + i;

		m = set_matrix_A_B(_pi);
		ret = cal_A_1(m);
		if(ret < 0) {
			total--;
			continue;
		}
		__get_x(m);
		*x += X.x;
		*y += X.y;
		sys_debug("%lf %lf\n", X.x, X.y);
	}
	if(total) {
		*x /= total;
		*y /= total;
	} else
		return -1;
	return 0;
}

/*
 * pld: dissipation
 * d0: sample point distance
 * pld0: d0 point dissipation
 * */
double distance(double pld, double d0, double pld0, double r)
{
	double x = (pld - pld0) / (10 * r);
	double y = pow(10, x);
	return d0 * y;
}
